#include "lk_motor.hpp"

lk_motor_5015::lk_motor_5015(rs485_v3 *Prs485, uint8_t motor_id) : _Prs485(Prs485), _motor_id(motor_id)
{
    init_flag = YJ_ERROR;
}

yj_return lk_motor_5015::lk_motor_5015_init(void)
{
    if (!_Prs485)
    {
        LOG(ERROR) << "❌ lk_motor_5015_init: _Prs485 is null";
        return init_flag;
    }
    if (_Prs485->rs485_open() != YJ_OK)
        return init_flag;
    init_flag = YJ_OK;
    return init_flag;
}

uint8_t lk_motor_5015::_calculate_checksum(uint8_t *data, uint8_t len)
{
    uint16_t checksum = 0;
    for (int i = 0; i < len; i++)
    {
        checksum += data[i];
    }
    checksum = checksum & 0x00FF;
    return checksum;
}

lk_motor_5015::~lk_motor_5015()
{
}

yj_return lk_motor_5015::_check_checksum(uint8_t *data, uint8_t len)
{

    uint16_t checksum = 0;
    for (int i = 0; i < len - 1; i++)
    {
        checksum += data[i];
    }
    checksum = checksum & 0x00FF;
    if (checksum != data[len - 1])
        return YJ_ERROR;
    else
        return YJ_OK;
}

std::vector<uint8_t> lk_motor_5015::_build_send_cmd_frame(lk_motor_5015_cmd_en cmd)
{
    std::vector<uint8_t> msg;
    msg.push_back(0x3E);
    msg.push_back((uint8_t)cmd);
    msg.push_back(_motor_id);
    // 确定发送数据帧长度
    switch (cmd)
    {
    case LK_MOTOR_5015_CMD_READ_STATUS_1:
    case LK_MOTOR_5015_CMD_CLEAR_ERROR:
    case LK_MOTOR_5015_CMD_CLOSE:
    case LK_MOTOR_5015_CMD_OPEN:
    case LK_MOTOR_5015_CMD_STOP:
    case LK_MOTOR_5015_CMD_SET_ENCODER_ZERO_TO_FLASH:
    case LK_MOTOR_5015_CMD_READ_MUTI_CIRCLES_ANGLE:
    case LK_MOTOR_5015_CMD_CLEAR_MUTI_CIRCLES_ANGLE:
        msg.push_back(0x00);
        break;
    case LK_MOTOR_5015_CMD_MUTIL_CIRCLES_MODE_2:
        msg.push_back(0x0C);
        break;
    }
    msg.push_back(_calculate_checksum(msg.data(), msg.size()));
    return msg;
}

yj_return lk_motor_5015::lk_motor_5015_muti_circle_angle_control_2(float angle, float max_speed)
{
    std::vector<uint8_t> recv_msg, send_msg;
    std::vector<uint8_t> msg, msg2;
    if (init_flag != YJ_OK)
    {
        LOG(ERROR) << "❌ lk_motor_5015_muti_circle_angle_control: motor not init";
        return YJ_ERROR;
    }

    recv_msg.resize(13);

    int64_t angle_int = (int64_t)(angle * 100);
    int32_t max_speed_int = (int64_t)(max_speed * 100);

    // LOG(INFO) << "===angle: " << angle_int << " max_speed: " << max_speed_int;
    // LOG(INFO) << "===angle: " << std::hex << std::setw(16) << std::setfill('0') << angle_int
    //           << " max_speed: " << std::hex << std::setw(8) << std::setfill('0') << max_speed_int;
    msg = _build_send_cmd_frame(LK_MOTOR_5015_CMD_MUTIL_CIRCLES_MODE_2);

    msg2.push_back(((uint8_t *)&angle_int)[0]);
    msg2.push_back(((uint8_t *)&angle_int)[1]);
    msg2.push_back(((uint8_t *)&angle_int)[2]);
    msg2.push_back(((uint8_t *)&angle_int)[3]);
    msg2.push_back(((uint8_t *)&angle_int)[4]);
    msg2.push_back(((uint8_t *)&angle_int)[5]);
    msg2.push_back(((uint8_t *)&angle_int)[6]);
    msg2.push_back(((uint8_t *)&angle_int)[7]);
    msg2.push_back(((uint8_t *)&max_speed_int)[0]);
    msg2.push_back(((uint8_t *)&max_speed_int)[1]);
    msg2.push_back(((uint8_t *)&max_speed_int)[2]);
    msg2.push_back(((uint8_t *)&max_speed_int)[3]);

    msg2.push_back(_calculate_checksum(msg2.data(), msg2.size()));

    send_msg.insert(send_msg.end(), msg.begin(), msg.end());
    send_msg.insert(send_msg.end(), msg2.begin(), msg2.end());

    // 打印发送的数据

    if (_Prs485->rs485_send(send_msg.data(), send_msg.size()) != send_msg.size())
    {
        LOG(ERROR) << "❌ lk_motor_5015_muti_circle_angle_control: motor send cmd error";
        _Prs485->rs485_flush_all();
        return YJ_ERROR;
    }

    if (_Prs485->rs485_recv(recv_msg.data(), recv_msg.size()) != recv_msg.size())
    // if (_Prs485->rs485_recv_with_length(recv_msg.data(), recv_msg.size()) <= 0)
    {
        LOG(ERROR) << "❌ lk_motor_5015_muti_circle_angle_control: motor recv cmd error";
        _Prs485->rs485_flush_all();
        return YJ_ERROR;
    }

    // LOG(INFO) << "recv size: "<< (int)recv_msg.size();
    // for (int i = 0; i < recv_msg.size(); i++)
    // {
    //     LOG(INFO) << "recv_msg[" << i << "]: " << std::hex << std::setw(2) << std::setfill('0') << (int)recv_msg[i];
    // }

    if (_check_checksum(recv_msg.data(), msg.size()) != YJ_OK || _check_checksum(recv_msg.data() + msg.size(), 8) != YJ_OK)
    {

        LOG(ERROR) << "❌ lk_motor_5015_muti_circle_angle_control: motor recv cmd checksum error";
        return YJ_ERROR;
    }
    return YJ_OK;
}

yj_return lk_motor_5015::lk_motor_5015_set_encoder_zero_to_flash()
{
    std::vector<uint8_t> recv_msg, send_msg;
    if (init_flag != YJ_OK)
    {
        LOG(ERROR) << "❌ lk_motor_5015_set_encoder_zero_to_flash: motor not init";
        return YJ_ERROR;
    }

    recv_msg.resize(8);
    send_msg = _build_send_cmd_frame(LK_MOTOR_5015_CMD_SET_ENCODER_ZERO_TO_FLASH);

    if (_Prs485->rs485_send(send_msg.data(), send_msg.size()) != send_msg.size())
    {
        LOG(ERROR) << "❌ lk_motor_5015_set_encoder_zero_to_flash: motor send cmd error";
        return YJ_ERROR;
    }

    if (_Prs485->rs485_recv(recv_msg.data(), recv_msg.size()) != recv_msg.size())
    // if (_Prs485->rs485_recv_with_length(recv_msg.data(), recv_msg.size()) <= 0)
    {
        LOG(ERROR) << "❌ lk_motor_5015_set_encoder_zero_to_flash: motor recv cmd error";
        return YJ_ERROR;
    }

    if (_check_checksum(recv_msg.data(), 5) != YJ_OK ||
        _check_checksum(recv_msg.data() + 5, 3) != YJ_OK)
    {
        LOG(ERROR) << "❌ lk_motor_5015_set_encoder_zero_to_flash: motor recv cmd checksum error";
        return YJ_ERROR;
    }

    uint16_t encoder_now;
    encoder_now = (recv_msg[7] << 8) | recv_msg[6];
    LOG(INFO) << "✅ lk_motor_5015_set_encoder_zero_to_flash: motor encoder now is " << encoder_now;

    return YJ_OK;
}

yj_return lk_motor_5015::lk_motor_5015_read_status_1(Plk_motor_5015_status_1_t status_1)
{
    std::vector<uint8_t> recv_msg, send_msg;
    if (init_flag != YJ_OK)
    {
        LOG(ERROR) << "❌ lk_motor_5015_read_status_1: motor not init";
        return YJ_ERROR;
    }
    if (!status_1)
    {
        LOG(ERROR) << "❌ lk_motor_5015_read_status_1: status_1 angle is null";
        return YJ_ERROR;
    }

    recv_msg.resize(13);
    send_msg = _build_send_cmd_frame(LK_MOTOR_5015_CMD_READ_STATUS_1);

    if (_Prs485->rs485_send(send_msg.data(), send_msg.size()) != send_msg.size())
    {
        LOG(ERROR) << "❌ lk_motor_5015_read_status_1: motor send cmd error";
        return YJ_ERROR;
    }

    if (_Prs485->rs485_recv(recv_msg.data(), recv_msg.size()) != recv_msg.size())
    // if (_Prs485->rs485_recv_with_length(recv_msg.data(), recv_msg.size()) <= 0)
    {
        LOG(ERROR) << "❌ lk_motor_5015_read_status_1: motor recv cmd error";
        return YJ_ERROR;
    }

    if (_check_checksum(recv_msg.data(), 5) != YJ_OK ||
        _check_checksum(recv_msg.data() + 5, 8) != YJ_OK)
    {
        LOG(ERROR) << "❌ lk_motor_5015_read_status_1: motor recv cmd checksum error";
        return YJ_ERROR;
    }

    status_1->temperature = recv_msg[5];
    float temp = (float)(recv_msg[6] | (recv_msg[7] << 8));
    status_1->voltage = temp / 100.0;
    status_1->status = recv_msg[10];
    status_1->error = recv_msg[11];
    LOG(INFO) << "✅ lk_motor_5015_read_status_1:\n"
              << "temperature: " << (int)status_1->temperature << "℃\n"
              << "voltage: " << status_1->voltage << "V\n"
              << "status: " << (int)status_1->status << "\n"
              << "error: 0x" << std::hex << std::setw(2) << std::setfill('0') << status_1->error;
    return YJ_OK;
}
yj_return lk_motor_5015::lk_motor_5015_clear_error()
{
    lk_motor_5015_status_1_t status_1;
    std::vector<uint8_t> recv_msg, send_msg;
    if (init_flag != YJ_OK)
    {
        LOG(ERROR) << "❌ lk_motor_5015_clear_error: motor not init";
        return YJ_ERROR;
    }

    recv_msg.resize(13);
    send_msg = _build_send_cmd_frame(LK_MOTOR_5015_CMD_READ_STATUS_1);

    if (_Prs485->rs485_send(send_msg.data(), send_msg.size()) != send_msg.size())
    {
        LOG(ERROR) << "❌ lk_motor_5015_clear_error: motor send cmd error";
        return YJ_ERROR;
    }

    if (_Prs485->rs485_recv(recv_msg.data(), recv_msg.size()) != recv_msg.size())
    // if (_Prs485->rs485_recv_with_length(recv_msg.data(), recv_msg.size()) <= 0)
    {
        LOG(ERROR) << "❌ lk_motor_5015_clear_error: motor recv cmd error";
        return YJ_ERROR;
    }

    if (_check_checksum(recv_msg.data(), 5) != YJ_OK ||
        _check_checksum(recv_msg.data() + 5, 8) != YJ_OK)
    {
        LOG(ERROR) << "❌ lk_motor_5015_clear_error: motor recv cmd checksum error";
        return YJ_ERROR;
    }

    status_1.temperature = recv_msg[5];
    float temp = (float)(recv_msg[6] | (recv_msg[7] << 8));
    status_1.voltage = temp / 100.0;
    status_1.status = recv_msg[10];
    status_1.error = recv_msg[11];
    LOG(INFO) << "✅ lk_motor_5015_clear_error:\n"
              << "temperature: " << (int)status_1.temperature << "℃\n"
              << "voltage: " << status_1.voltage << "V\n"
              << "status: " << (int)status_1.status << "\n"
              << "error: 0x" << std::hex << std::setw(2) << std::setfill('0') << status_1.error;
    return YJ_OK;
}

yj_return lk_motor_5015::lk_motor_5015_read_muti_circle_angle(float *angle)
{
    std::vector<uint8_t> recv_msg, send_msg;
    if (init_flag != YJ_OK)
    {
        LOG(ERROR) << "❌ lk_motor_5015_read_muti_circle_angle: motor not init";
        return YJ_ERROR;
    }

    if (!angle)
    {
        LOG(ERROR) << "❌ lk_motor_5015_read_muti_circle_angle: input angle is null";
        return YJ_ERROR;
    }

    recv_msg.resize(14);
    send_msg = _build_send_cmd_frame(LK_MOTOR_5015_CMD_READ_MUTI_CIRCLES_ANGLE);

    if (_Prs485->rs485_send(send_msg.data(), send_msg.size()) != send_msg.size())
    {
        LOG(ERROR) << "❌ lk_motor_5015_read_muti_circle_angle: motor send cmd error";
        return YJ_ERROR;
    }

    if (_Prs485->rs485_recv(recv_msg.data(), recv_msg.size()) != recv_msg.size())
    // if (_Prs485->rs485_recv_with_length(recv_msg.data(), recv_msg.size()) <= 0)
    {
        LOG(ERROR) << "❌ lk_motor_5015_read_muti_circle_angle: motor recv cmd error";
        return YJ_ERROR;
    }

    if (_check_checksum(recv_msg.data(), 5) != YJ_OK ||
        _check_checksum(recv_msg.data() + 5, 9) != YJ_OK)
    {
        LOG(ERROR) << "❌ lk_motor_5015_read_muti_circle_angle: motor recv cmd checksum error";
        return YJ_ERROR;
    }

    int64_t angle_read =
        ((int64_t)recv_msg[12] << 56) | ((int64_t)recv_msg[11] << 48) | ((int64_t)recv_msg[10] << 40) |
        ((int64_t)recv_msg[9] << 32) | ((int64_t)recv_msg[8] << 24) | ((int64_t)recv_msg[7] << 16) |
        ((int64_t)recv_msg[6] << 8) | ((int64_t)recv_msg[5]);

    *angle = (float)angle_read / 100; // 角度单位为度

    // LOG(INFO) << "✅ lk_motor_5015_read_muti_circle_angle: motor encoder now is " << angle_read;
    return YJ_OK;
}

yj_return lk_motor_5015::lk_motor_5015_clear_muti_circle_angle()
{
    std::vector<uint8_t> recv_msg, send_msg;
    if (init_flag != YJ_OK)
    {
        LOG(ERROR) << "❌ lk_motor_5015_clear_muti_circle_angle: motor not init";
        return YJ_ERROR;
    }

    recv_msg.resize(5);
    send_msg = _build_send_cmd_frame(LK_MOTOR_5015_CMD_CLEAR_MUTI_CIRCLES_ANGLE);

    if (_Prs485->rs485_send(send_msg.data(), send_msg.size()) != send_msg.size())
    {
        LOG(ERROR) << "❌ lk_motor_5015_clear_muti_circle_angle: motor send cmd error";
        return YJ_ERROR;
    }

    if (_Prs485->rs485_recv(recv_msg.data(), recv_msg.size()) != recv_msg.size())
    // if (_Prs485->rs485_recv_with_length(recv_msg.data(), recv_msg.size()) <= 0)
    {
        LOG(ERROR) << "❌ lk_motor_5015_clear_muti_circle_angle: motor recv cmd error";
        return YJ_ERROR;
    }

    if (_check_checksum(recv_msg.data(), 5) != YJ_OK)
    {
        LOG(ERROR) << "❌ lk_motor_5015_read_muti_circle_angle: motor recv cmd checksum error";
        return YJ_ERROR;
    }
    return YJ_OK;
}

yj_return lk_motor_5015::lk_motor_5015_close()
{
    std::vector<uint8_t> recv_msg, send_msg;
    if (init_flag != YJ_OK)
    {
        LOG(ERROR) << "❌ lk_motor_5015_close: motor not init";
        return YJ_ERROR;
    }

    recv_msg.resize(5);

    send_msg = _build_send_cmd_frame(LK_MOTOR_5015_CMD_CLOSE);
    if (_Prs485->rs485_send(send_msg.data(), send_msg.size()) != send_msg.size())
    {
        LOG(ERROR) << "❌ lk_motor_5015_close: motor send cmd error";
        return YJ_ERROR;
    }

    if (_Prs485->rs485_recv(recv_msg.data(), recv_msg.size()) != recv_msg.size())
    // if (_Prs485->rs485_recv_with_length(recv_msg.data(), recv_msg.size()) <= 0)
    {
        LOG(ERROR) << "❌ lk_motor_5015_close: motor recv cmd error";
        return YJ_ERROR;
    }

    if (_check_checksum(recv_msg.data(), recv_msg.size()) != YJ_OK)
    {
        LOG(ERROR) << "❌ lk_motor_5015_close: motor recv cmd checksum error";
        return YJ_ERROR;
    }
    return YJ_OK;
}

yj_return lk_motor_5015::lk_motor_5015_open()
{
    std::vector<uint8_t> recv_msg, send_msg;
    if (init_flag != YJ_OK)
    {
        LOG(ERROR) << "❌ lk_motor_5015_open: motor not init";
        return YJ_ERROR;
    }

    recv_msg.resize(5);

    send_msg = _build_send_cmd_frame(LK_MOTOR_5015_CMD_OPEN);
    if (_Prs485->rs485_send(send_msg.data(), send_msg.size()) != send_msg.size())
    {
        LOG(ERROR) << "❌ lk_motor_5015_open: motor send cmd error";
        return YJ_ERROR;
    }

    if (_Prs485->rs485_recv(recv_msg.data(), recv_msg.size()) != recv_msg.size())
    // if (_Prs485->rs485_recv_with_length(recv_msg.data(), recv_msg.size()) <= 0)
    {
        LOG(ERROR) << "❌ lk_motor_5015_open: motor recv cmd error";
        return YJ_ERROR;
    }

    if (_check_checksum(recv_msg.data(), recv_msg.size()) != YJ_OK)
    {
        LOG(ERROR) << "❌ lk_motor_5015_open: motor recv cmd checksum error";
        return YJ_ERROR;
    }
    return YJ_OK;
}

yj_return lk_motor_5015::lk_motor_5015_stop()
{
    std::vector<uint8_t> recv_msg, send_msg;
    if (init_flag != YJ_OK)
    {
        LOG(ERROR) << "❌ lk_motor_5015_stop: motor not init";
        return YJ_ERROR;
    }

    recv_msg.resize(5);

    send_msg = _build_send_cmd_frame(LK_MOTOR_5015_CMD_STOP);
    if (_Prs485->rs485_send(send_msg.data(), send_msg.size()) != send_msg.size())
    {
        LOG(ERROR) << "❌ lk_motor_5015_stop: motor send cmd error";
        return YJ_ERROR;
    }

    if (_Prs485->rs485_recv(recv_msg.data(), recv_msg.size()) != recv_msg.size())
    // if (_Prs485->rs485_recv_with_length(recv_msg.data(), recv_msg.size()) <= 0)
    {
        LOG(ERROR) << "❌ lk_motor_5015_stop: motor recv cmd error";
        return YJ_ERROR;
    }

    if (_check_checksum(recv_msg.data(), recv_msg.size()) != YJ_OK)
    {
        LOG(ERROR) << "❌ lk_motor_5015_stop: motor recv cmd checksum error";
        return YJ_ERROR;
    }
    return YJ_OK;
}

yj_return lk_motor_5015::lk_motor_5015_move_to_muti_circle_angle_and_wait(float angle, float max_speed, float angle_tollerance, uint64_t wait_time_ms)
{
    if (init_flag != YJ_OK)
    {
        LOG(ERROR) << "❌ lk_motor_5015_move_to_muti_circle_angle_and_wait: motor not init";
        return YJ_ERROR;
    }
    uint32_t error_times = 0;
    float angle_temp;
    struct timespec ts;

    uint64_t start_time, end_time;

    while (lk_motor_5015_muti_circle_angle_control_2(angle, max_speed) != YJ_OK && error_times < 20)
    {
        error_times++;
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
    if (error_times >= 20)
    {
        LOG(ERROR) << "❌ lk_motor_5015_move_to_muti_circle_angle_and_wait: motor move to angle error";
        return YJ_ERROR;
    }

    clock_gettime(CLOCK_REALTIME, &ts); // 获取系统实时时间
    start_time = (uint64_t)ts.tv_sec * 1000 + ts.tv_nsec / 1000000;
    do
    {
        error_times = 0;
        while (lk_motor_5015_read_muti_circle_angle(&angle_temp) != YJ_OK && error_times < 20)
        {
            error_times++;
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }

        clock_gettime(CLOCK_REALTIME, &ts); // 获取系统实时时间
        end_time = (uint64_t)ts.tv_sec * 1000 + ts.tv_nsec / 1000000;

    } while (abs(angle_temp - angle) > angle_tollerance && (end_time - start_time) < wait_time_ms);

    if (abs(angle_temp - angle) > angle_tollerance)
    {
        LOG(ERROR) << "❌ lk_motor_5015_move_to_muti_circle_angle_and_wait: motor can't reach the angle until timeout";
        return YJ_ERROR;
    }

    return YJ_OK;
}